/**
  ****************************(C) COPYRIGHT 2019 DJI****************************
  * @file       can_receive.c/h
  * @brief      这里是CAN中断接收函数，接收电机数据,CAN发送函数发送电机电流控制电机.
  * @note       
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-26-2018     RM              1. done
  *  V1.1.0     Nov-11-2019     RM              1. support hal lib
  *  V1.2.0     11-25- 2021     yanfubing       1.add engineer can2
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2019 DJI****************************
  */
#include "CAN_receive.h"
#include "cmsis_os.h"
#include "main.h"
#include "bsp_rng.h"
#include "detect_task.h"
#include "remote_control.h"
#include "ins_task.h"
#include "fall_down_task.h"
#include "small_black_box.h"

#include <string.h>
/*******************CAN通讯*******************/

extern CAN_HandleTypeDef hcan1;      
extern CAN_HandleTypeDef hcan2;
//motor data read
//获取电机的数据
#define get_motor_measure(ptr, data)                                    \
    {                                                                   \
        (ptr)->last_ecd = (ptr)->ecd;                                   \
        (ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);            \
        (ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]);      \
        (ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]);  \
        (ptr)->temperate = (data)[6];                                   \
    }

/*
电机数据:   
		     CAN1							
    0：底盘电机1 3508电机,  
    1：底盘电机2 3508电机,
    2：底盘电机3 3508电机,
    3：底盘电机4 3508电机;
		4：升降电机左 3508电机;
		5：升降电机右 3508电机;
    6：救援卡电机 2006电机
	       CAN2
		0：障碍电机左 3508电机；
		1：障碍电机右 3508电机;
		2: 救援电机左 2006电机;
		3: 救援电机右 2006电机;	
*/
static motor_measure_t motor_chassis[6];
static motor_measure_t motor_engineer[4];
static motor_measure_t motor_rescue_IDcaed[1];                //救援卡电机数据 by yan fubing

static CAN_TxHeaderTypeDef  chassis_tx_message;
static uint8_t              chassis_can_send_data[8];         //底盘电机发送的数据
static CAN_TxHeaderTypeDef  Go_Down_tx_message;
static uint8_t              Go_Down_can_send_data[8];         //升降电机数据发送的数据
static CAN_TxHeaderTypeDef  obstacle_rescue_tx_message;
static uint8_t              obstacle_rescue_can_send_data[8]; //救援 ，障碍块发送的数据
  
static CAN_TxHeaderTypeDef  can2_send;
static uint8_t              can2_send_date[8];                //底盘CAN2发送给云台的数据

static can2_receive_t can2_receive_rc_data;

 rm_imu_data_t rm_imu_data;
/**
  * @brief          hal库CAN回调函数,接收电机数据
  * @param[in]      hcan:CAN句柄指针
  * @retval         none
  */
	
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{      
    CAN_RxHeaderTypeDef rx_header;
      uint8_t rx_data[8];
    HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data);
    if (hcan == &CHASSIS_CAN)
    {
        switch (rx_header.StdId)
       {
        case CAN_chassis_3508_M1_ID:
        case CAN_chassis_3508_M2_ID:
        case CAN_chassis_3508_M3_ID:
        case CAN_chassis_3508_M4_ID:
        {
				static uint8_t i = 0;
				i = rx_header.StdId - CAN_chassis_3508_M1_ID;
			 	get_motor_measure(&motor_chassis[i], rx_data);
         detect_hook(CHASSIS_MOTOR1_TOE + i);
								break;
				}
				case CAN_go_down_3508_M1_ID:
				case CAN_go_down_3508_M2_ID:
				{
					static uint8_t i = 0;
					i = rx_header.StdId - CAN_go_down_3508_M1_ID + 4;
					get_motor_measure(&motor_chassis[i], rx_data);
       if (motor_chassis[i].ecd - motor_chassis[i].last_ecd> 4096)
				{
						motor_chassis[i].ecd_count--;
				}
				else if (motor_chassis[i].ecd - motor_chassis[i].last_ecd<-4096)
				{
						motor_chassis[i].ecd_count++;
				}
					detect_hook(CHASSIS_MOTOR1_TOE + i);
					break;
				}
				 case CAN_rescue_IDcard_ID:
        {
      	get_motor_measure(&motor_rescue_IDcaed[0], rx_data);
           if (motor_rescue_IDcaed[0].ecd - motor_rescue_IDcaed[0].last_ecd> 4096)
				{
						motor_rescue_IDcaed[0].ecd_count--;
				}
				else if (motor_rescue_IDcaed[0].ecd - motor_rescue_IDcaed[0].last_ecd<-4096)
				{
						motor_rescue_IDcaed[0].ecd_count++;
				}
          detect_hook(rescue_IDcard_motor_TOE);
          break;
        }
        default:
        {
            break;
        }
       }
     }
		
    else if (hcan == &engineering_CAN)    
    {
        switch (rx_header.StdId)
        {
	
		case CAN_obstacle_3508_M1_ID:
		case CAN_obstacle_3508_M2_ID:
		case CAN_rescue_3508_M1_ID:
		case CAN_rescue_3508_M2_ID:
        {
					static uint8_t i = 0;
					i = rx_header.StdId - CAN_obstacle_3508_M1_ID;
					get_motor_measure(&motor_engineer[i], rx_data);
				if(motor_engineer[i].ecd - motor_engineer[i].last_ecd> 4096)
				{
						motor_engineer[i].ecd_count--;
				}
				else if(motor_engineer[i].ecd - motor_engineer[i].last_ecd<-4096)
				{
						motor_engineer[i].ecd_count++;
				}
				detect_hook(obstacle_motor1_TOE + i);
					break;
        }
  	case CAN_receive_RC_ID:
        {
				//获取遥控数据
				can2_receive_rc_data.x_speed=rx_data[0];
				can2_receive_rc_data.y_speed=rx_data[1];
				can2_receive_rc_data.z_speed=rx_data[2];					
				can2_receive_rc_data.fall_down_speed    = rx_data[3];
				can2_receive_rc_data.chassis_state_flag =(rx_data[4] >> 5) & 0x07;   //高三位对应底盘的6种状态，
				can2_receive_rc_data.engineer_work_flg  =(rx_data[4] >> 3) & 0x03;   //4、5位对应三种工作模式：救援，取矿，兑换
				can2_receive_rc_data.cail_flag          =(rx_data[5] >> 6) & 0x03;   //高两位，0x7F对应底盘救援校准，0xBF底盘校准
        can2_receive_rc_data.key                =((uint16_t)(rx_data[6]<<8 ) |rx_data[7]);	 //键盘控制数据
				detect_hook(gimbal_can_TOE);
				break;
		    }	
	
	  case RM_IMU_PARAM_ID:
			 {
			 rm_imu_data.accel_rangle = rx_data[0] &0x0F;
			 rm_imu_data.gyro_rangle = (rx_data[0] &0xF0) >> 4;
			 rm_imu_data.sensor_control_temperature = rx_data[2];
			 rm_imu_data.imu_sensor_rotation = rx_data[3] & 0x1F;
			 rm_imu_data.ahrs_rotation_sequence = (rx_data[3] & 0xE0) >> 5;
			 rm_imu_data.quat_euler = rx_data[4] & 0x01;
			 switch(rm_imu_data.gyro_rangle)
			 {
			 case 0: rm_imu_data.gyro_sen = GYRO_2000_SEN; break;
			 case 1: rm_imu_data.gyro_sen = GYRO_1000_SEN; break;
			 case 2: rm_imu_data.gyro_sen = GYRO_500_SEN; break;
			 case 3: rm_imu_data.gyro_sen = GYRO_250_SEN; break;
			 case 4: rm_imu_data.gyro_sen = GYRO_125_SEN; break;
			 }
			 switch(rm_imu_data.accel_rangle)
			 {
			 case 0: rm_imu_data.accel_sen = ACCEL_3G_SEN; break;
			 case 1: rm_imu_data.accel_sen = ACCEL_6G_SEN; break;
			 case 2: rm_imu_data.accel_sen = ACCEL_12G_SEN; break;
			 case 3: rm_imu_data.accel_sen = ACCEL_24G_SEN; break;
			 }
			 break;
			 }
			 	case RM_IMU_QUAT_ID:
			 {
			 if(rm_imu_data.quat_euler && rx_header.DLC == 6)
			 {
			 memcpy(rm_imu_data.euler_angle,rx_data, 6);
			 rm_imu_data.euler_angle_fp32[0] = rm_imu_data.euler_angle[0] * 0.0001f;
			 rm_imu_data.euler_angle_fp32[1] = rm_imu_data.euler_angle[1] * 0.0001f;
			 rm_imu_data.euler_angle_fp32[2] = rm_imu_data.euler_angle[2] * 0.0001f;
			 }
			 else if(rm_imu_data.quat_euler == 0 &&rx_header.DLC == 8)
			 {
			 memcpy(rm_imu_data.quat, rx_data, 8);
			 rm_imu_data.quat_fp32[0] = rm_imu_data.quat[0] * 0.0001f;
			 rm_imu_data.quat_fp32[1] = rm_imu_data.quat[1] * 0.0001f;
			 rm_imu_data.quat_fp32[2] = rm_imu_data.quat[2] * 0.0001f;
			 rm_imu_data.quat_fp32[3] = rm_imu_data.quat[3] * 0.0001f;
			 }
			 break;
			 }
			 case RM_IMU_GYRO_ID:
			 {
			 memcpy(rm_imu_data.gyro_int16, rx_data,6);
			 rm_imu_data.gyro_fp32[0] = rm_imu_data.gyro_int16[0] * rm_imu_data.gyro_sen;
			 rm_imu_data.gyro_fp32[1] = rm_imu_data.gyro_int16[1] * rm_imu_data.gyro_sen;
			 rm_imu_data.gyro_fp32[2] = rm_imu_data.gyro_int16[2] * rm_imu_data.gyro_sen;
			 rm_imu_data.sensor_temperature = (int16_t)((rx_data[6] << 3) | (rx_data[7] >> 
			5));
			 if (rm_imu_data.sensor_temperature > 1023)
			 {
			 rm_imu_data.sensor_temperature -= 2048;
			 }
			 break;
			 }
			 case RM_IMU_ACCEL_ID:
			 {
			 memcpy(rm_imu_data.accel_int16, rx_data,6);
			 rm_imu_data.accel_fp32[0] = rm_imu_data.accel_int16[0] * rm_imu_data.accel_sen;
			 rm_imu_data.accel_fp32[1] = rm_imu_data.accel_int16[1] * rm_imu_data.accel_sen;
			 rm_imu_data.accel_fp32[2] = rm_imu_data.accel_int16[2] * rm_imu_data.accel_sen;
			 memcpy(&rm_imu_data.sensor_time, (rx_data+ 6), 2);
			 break;
			 }
			 case RM_IMU_MAG_ID:
			 {
			 memcpy(rm_imu_data.mag_int16, rx_data,6);
			 break;
			 }

		
        default:
        {
           break;
        }        
			  }
		} 
}


/**
  * @brief          发送ID为0x700的CAN包,它会设置chassis3508电机进入快速设置ID
  * @param[in]      none
  * @retval         none
  */
void CAN_cmd_chassis_reset_ID(void)
{
    uint32_t send_mail_box;
    chassis_tx_message.StdId = 0x700;
    chassis_tx_message.IDE = CAN_ID_STD;
    chassis_tx_message.RTR = CAN_RTR_DATA;
    chassis_tx_message.DLC = 0x08;
    chassis_can_send_data[0] = 0;
    chassis_can_send_data[1] = 0;
    chassis_can_send_data[2] = 0;
    chassis_can_send_data[3] = 0;
    chassis_can_send_data[4] = 0;
    chassis_can_send_data[5] = 0;
    chassis_can_send_data[6] = 0;
    chassis_can_send_data[7] = 0;

    HAL_CAN_AddTxMessage(&CHASSIS_CAN, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}


/*******************CAN_1发送电流***********************************************************/
/**
  * @brief          发送电机控制电流(0x201,0x202,0x203,0x204)
  * @param[in]      motor1: (0x201) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      motor2: (0x202) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      motor3: (0x203) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      motor4: (0x204) 3508电机控制电流, 范围 [-16384,16384]
  * @retval         none
  */
void CAN_cmd_chassis(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)
{
    uint32_t send_mail_box;
    chassis_tx_message.StdId = CAN_CHASSIS_ALL_ID;
    chassis_tx_message.IDE = CAN_ID_STD;
    chassis_tx_message.RTR = CAN_RTR_DATA;
    chassis_tx_message.DLC = 0x08;
    chassis_can_send_data[0] = motor1 >> 8;
    chassis_can_send_data[1] = motor1;
    chassis_can_send_data[2] = motor2 >> 8;
    chassis_can_send_data[3] = motor2;
    chassis_can_send_data[4] = motor3 >> 8;
    chassis_can_send_data[5] = motor3;
    chassis_can_send_data[6] = motor4 >> 8;
    chassis_can_send_data[7] = motor4;
     
    HAL_CAN_AddTxMessage(&CHASSIS_CAN, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}

/**
  * @brief          发送升降电机控制电流(0x205,0x206) &&!!!救援卡的电机也挂在上面
  * @param[in]      motor_1: (0x205) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      motor_2: (0x206) 3508电机控制电流, 范围 [-16384,16384]
  * @retval         none
  */
void CAN_cmd_go_down(int16_t motor_1, int16_t motor_2,int16_t rescue_ID_Card)
{
	uint32_t send_mail_box;
	Go_Down_tx_message.StdId = CAN_GO_DOWN_ALL_ID;
	Go_Down_tx_message.IDE = CAN_ID_STD;
	Go_Down_tx_message.RTR = CAN_RTR_DATA;
	Go_Down_tx_message.DLC = 0x06;
	Go_Down_can_send_data[0] = motor_1 >> 8;
	Go_Down_can_send_data[1] = motor_1;
	Go_Down_can_send_data[2] = motor_2 >> 8;
	Go_Down_can_send_data[3] = motor_2;
	Go_Down_can_send_data[4] = rescue_ID_Card >> 8;
	Go_Down_can_send_data[5] = rescue_ID_Card;
	Go_Down_can_send_data[6] = 0;
	Go_Down_can_send_data[7] = 0;

	HAL_CAN_AddTxMessage(&CHASSIS_CAN, &Go_Down_tx_message, Go_Down_can_send_data, &send_mail_box);
}

/**
  * @brief          发送障碍块 救援电机控制电流(0x207,0x208)
  * @param[in]      motor_1: (0x207) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      motor_2: (0x208) 3508电机控制电流, 范围 [-16384,16384]
					motor_3   (0x209)
					motor_4   (0x20a)
  * @retval         none
  */ 
/*******************CAN_2发送电流***********************************************************/
void CAN_cmd_obstacle(int16_t motor_obstacle_1, int16_t motor_obstacle_2, int16_t motor_rescue_1, int16_t motor_rescue_2)
{
	uint32_t send_mail_box;
	obstacle_rescue_tx_message.StdId = CAN_obstacle_ALL_ID;
	obstacle_rescue_tx_message.IDE = CAN_ID_STD;
	obstacle_rescue_tx_message.RTR = CAN_RTR_DATA;
	obstacle_rescue_tx_message.DLC = 0x08;
	obstacle_rescue_can_send_data[0] = motor_obstacle_1 >> 8;
	obstacle_rescue_can_send_data[1] = motor_obstacle_1;
	obstacle_rescue_can_send_data[2] = motor_obstacle_2 >> 8;
	obstacle_rescue_can_send_data[3] = motor_obstacle_2;
	obstacle_rescue_can_send_data[4] = motor_rescue_1 >> 8;
	obstacle_rescue_can_send_data[5] = motor_rescue_1;
	obstacle_rescue_can_send_data[6] = motor_rescue_2 >> 8;
	obstacle_rescue_can_send_data[7] = motor_rescue_2;

	HAL_CAN_AddTxMessage(&engineering_CAN, &obstacle_rescue_tx_message, obstacle_rescue_can_send_data, &send_mail_box);
}


/**
  * @brief          底盘CAN2发送云台CAN2
  * @param[in]    
  * @param[in]     
  * @retval         none
  */
 void CAN2_Chassis_to_Gimbal(void)
 { 
	 uint32_t send_mail_box;
	 can2_send.StdId=CAN2_ID;
	 can2_send.IDE=CAN_ID_STD;
	 can2_send.RTR=CAN_RTR_DATA;
	 can2_send.DLC=0x02;
	 can2_send_date[0]=fall_down_move.Limit_Key_state[0] ;
   can2_send_date[1]=fall_down_move.Limit_Key_state[1] ;
   
   HAL_CAN_AddTxMessage(&engineering_CAN, &can2_send, can2_send_date, &send_mail_box);
 }

/**************************************************************************/
                                




/**************************************************************************/
const motor_measure_t *get_chassis_motor_measure_point(uint8_t i)
{
    return &motor_chassis[(i & 0x03)];
}
/**
  * @brief          返回升降电机 3508电机数据指针
  * @param[in]      i: 电机编号,范围[4，5]
  * @retval         电机数据指针
  */
 const motor_measure_t* get_fall_down_motor_measure_point(uint8_t i)
{
	return &motor_chassis[(i + 4)];
}

/**
  * @brief          返回障碍快电机 3508电机数据指针
  * @param[in]      i: 电机编号,范围[0,1]
  * @retval         电机数据指针
  */
 const motor_measure_t* get_obstacle_motor_measure_point(uint8_t i)
{
	return &motor_engineer[( i )];
}
/**
  * @brief          返回救援电机数据指针 3508
  * @param[in]      i: 电机编码，范围[0，1]
  * @retval         电机数据指针
  */
const motor_measure_t* get_rescue_motor_measure_point(uint8_t i)
{
	return &motor_engineer[(i + 2)];
}


/**
  * @brief          返回救援卡电机的数据指针
  * @param[in]      
  */
const motor_measure_t* get_rescue_IDcard_point()
{
  return &motor_rescue_IDcaed[0];
}

/**
  * @brief          返回CAN信号数据指针 
  * @param[in]      
  */
const can2_receive_t *get_can_control_point(void)
{
return &can2_receive_rc_data;
}
